/*============================================================================*/
/*                        Tortoise Team			                              */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         %APP_Machine_State.c%
* Instance:         RPL_1
* %version:         1.0 %
* %created_by:      Andres Torres Garcia %
* %date_created:    Sunday Sep  30 14:38:03 2012 %
*=============================================================================*/
/* DESCRIPTION : C source file for the machine state.                         */
/*============================================================================*/
/* FUNCTION COMMENT : This file contains the function and variable definitions*/
/*					  of the machine state                                    */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 30/09/2012  |                               | Andres Torres    */
/* Integration under Continuus CM                                             */
/*============================================================================*/

/* Includes */
/* -------- */
#include "APP_Machine_State.h"
#include "APPLeds.h"
#include "APP_Btn.h"
#include "APP_Linear_Camera.h"
#include "APP_BackWheels.h"
#include "APP_FrontWheels.h"
#include "APP_Knob.h"
#include "HAL_DcMotor.h"
#include "MAL_ADC.h"
#include "MAL_PWM.h"
#include "APP_Algorithm.h"

/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
/* Functions macros */

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 


/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/

/** \brief Pointer to function for the machine state. */
void (*ptrFctnMachineState[NUM_STATES]) (void);

/** \brief State of the machine state. */
enum STATE en_State = FIRST_STATE;

/** \brief Variable used to toggle the state of the back wheels. */
static T_UBYTE toggle = 1;

/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */

/* Private functions prototypes */
/* ---------------------------- */

void vfnFirstState(void);
void vfnSecondState(void);
void vfnThirdState(void);
void vfnFourState(void);

/* Exported functions prototypes */
/* ----------------------------- */

void vfnInitStates(void);

/* Inline functions */
/* ---------------- */

/* Private functions */
/* ----------------- */
/**************************************************************
 *  Name                 : vfnFirstState
 *  Description          : The first state of the machine state.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief The first state of the machine state.
/////////////////////////////////////////////////////////////////////////
void vfnFirstState(void)
{
	vfnCalibrate_FrontWheels();

	if(IsBtnStatePsh())
	{
		en_State = SECOND_STATE;
		
		vfnStartCapturing();
		
		APPLeds_Led1_app();
		APPLeds_Led2_app();
	}
}

/**************************************************************
 *  Name                 : vfnSecondState
 *  Description          :	The second state of the machine state.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief The second state of the machine state.
/////////////////////////////////////////////////////////////////////////
void vfnSecondState(void)
{
	if(IsBtnStatePsh())
	{
		en_State = THIRD_STATE;
	
		APPLeds_Led2_app();
		APPLeds_Led3_app();
	}
	
	vfnCalibrateLinearCamera();
}

/**************************************************************
 *  Name                 : vfnThirdState
 *  Description          : The third state of the machine state.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief The third state of the machine state.
/////////////////////////////////////////////////////////////////////////
void vfnThirdState(void)
{
	if(IsBtnStatePsh())
	{
		en_State = FOUR_STATE;
		
		ub_Can_Process_Pixels = 1;
		
		vfnStart_DcMotor(2);
		
		APPLeds_Led3_app();
		APPLeds_Led4_app();
	}
}

/**************************************************************
 *  Name                 : vfnFourState
 *  Description          : The four state of the machine state.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief The four state of the machine state.
/////////////////////////////////////////////////////////////////////////
void vfnFourState(void)
{
	if(vfnIsBtnTurnBackWheelsPsh())
	{	
		if(toggle)
		{
			toggle = 0;
			vfnStop_DcMotor(2);
		}
		else
		{
			toggle = 1;
			vfnStart_DcMotor(2);
		}
	}
	
	vfnFirstVersion();
	
	//Testing
	//vfnSetPos_FrontWheels(SM_KNOB(GET_KNOB_VALUE));
	
	//Put here the AI algorithm for the car.
	
}

/* Exported functions */
/* ------------------ */

/**************************************************************
 *  Name                 : vfnInitStates
 *  Description          : Initialize the pointer to function states.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Initialize the pointer to function states.
/////////////////////////////////////////////////////////////////////////
void vfnInitStates(void)
{
	ptrFctnMachineState[0] = &vfnFirstState;
	ptrFctnMachineState[1] = &vfnSecondState;
	ptrFctnMachineState[2] = &vfnThirdState;
	ptrFctnMachineState[3] = &vfnFourState;
}